/************************************************************************/
/* Controlling the serving process                                      */
/************************************************************************/

#include <avr/io.h>
#define F_CPU 8000000
#include <util/delay.h>
#include <avr/interrupt.h>
#include "lcd162.h"
#include "weight_sensor.h"
#include "engine.h"
#include "switch.h"
#include "serving_process.h"

//Stuck time = 0,52 s * STUCK_TIME:10 = 6 s
#define STUCK_TIME 10
#define STUCK_TIME_SERVING 20
// Remaining weight to add to the mix in which the
// engine will reduce its speed. (Should be 2 multiple)
#define WEIGHT_ALMOST 2

uint16_t ServeContainer(uint8_t product_number, int16_t product_weig);
uint8_t ServeMix();
void EngineCommand(uint8_t engine_num, uint8_t speed);
void StuckEnable(uint8_t enable);
uint8_t StuckChecker();
void Timer1Init();
uint8_t ErrorSell();

// Different to 0 if an error happens. Can be activated by the
// stop button S1 interrupt (bit STOP) or by the timer 1 overflow 
// interrupt (bit STUCK)
uint8_t error_sell = 0;
// Stuck checking counter. Incremented each timer 1 overflow
// if the weight hasn't increased.
uint8_t stuck_counter = 0;
// How many stuck checks will be before the is a stuck
uint8_t stuck_time = STUCK_TIME;
// Weight used to check any possible problem.
int16_t previous_weight = 0;

/* ------------------------------------------------------------
	Controlling the serving process
------------------------------------------------------------ */
uint16_t ServeContainer(uint8_t product_number, int16_t product_weight)
{
	// Weight of the container before the servings starts.
	int16_t container_weight = WeightRead();
	// Actual weight. Continuously updated.
	int16_t actual_weight = container_weight;
	uint8_t lcd_position = product_number * 4;
	
	// Phase 1: max speed until product almost added
	EngineCommand(product_number, SPEED_MAX);
	error_sell = 0;
	// Check weight until product it's almost served.	
	while ((actual_weight < (product_weight - WEIGHT_ALMOST + container_weight))&(error_sell == 0))
	{	
		actual_weight = WeightRead();
		LCDGotoXY(lcd_position,1);	
		LCDDispInteger(actual_weight - container_weight);
		LCDDispString(" ");
		_delay_ms(100);
	}

	// Phase 2: Reduced speed
	if (error_sell == 0)
	{
		EngineCommand(product_number, SPEED_25);	
	}	

	// Check weight until product is served	
	while ((actual_weight < (product_weight + container_weight))&(error_sell == 0))
	{
		actual_weight = WeightRead();
		LCDGotoXY(lcd_position,1);
		LCDDispInteger(actual_weight - container_weight);
		LCDDispString(" ");
		_delay_ms(100);
	}	
	
	// Stop the motor
	EngineCommand(product_number, SPEED_0);
		
	return (WeightRead() - container_weight);
}

/* ------------------------------------------------------------
	Serving the mix
------------------------------------------------------------ */

uint8_t ServeMix()
{			
	// Weight of the container before the servings starts.
	int16_t container_weight = WeightRead();
	// Temporal variable to help show the serving information
	// on the screen
	uint8_t percentage = 0;
	// Actual weight. Continuously updated.
	int16_t actual_weight = container_weight;
	
	// If everything is correct, start serving the product	
	if (error_sell == 0)
	{
		LCDClear();
		LCDDispString("Serving...      ");
		// Start the serving engine.
		EngineCommand(CONTAINER_MIX,SPEED_MAX);
	}

	// Wait until the product is served	
	while ((actual_weight > 0 ) & (error_sell == 0))
	{
		actual_weight = WeightRead();
		// Display info in the screen.
		LCDGotoXY(12,0);
		LCDDispString("  ");
		LCDDispInteger(actual_weight);
		LCDGotoXY(0,1);
		percentage = ( 9.5 -((10*actual_weight)/container_weight));
		if (percentage > 10)
		{
			percentage = 0;
		}
		LCDDispInteger(percentage * 10);
		LCDDispString("% ");
		LCDGotoXY(6,1);
		for (uint8_t j = 0; j < percentage; j++)
		{
			LCDDispChar(0xFF); // Black character.
		}
		
		_delay_ms(100);
	}
	
	if (error_sell == 0)
	{
		// Ensure that all the product has been served by
		// waiting 1 more second.
		_delay_ms(1000);
		LCDClear();
		LCDDispString("-----Served-----");
	}
	//Stop the motor
	EngineCommand(CONTAINER_MIX, SPEED_0);									
	return error_sell;
}

/* ------------------------------------------------------------
	Enabling/disabling timer 1 overflow interrupt and
	restarts the timer when enabled.
-------------------------------------------------------------- */
void StuckEnable(uint8_t enable)
{
	if (enable) 
	{
		// Restart stuck_counter
		stuck_counter = 0;		
		// Restart Timer1
		TCNT1 = 0;	
		// Clear timer 1 overflow flag
		TIFR = 1 << TOV1;
		// Enable interrupt
		TIMSK |= (1<<TOIE1);	
	}
	else // Disable interrupt
	{
		TIMSK &= (~(1<<TOIE1));
	}				
}

/* ------------------------------------------------------------
	Controlling the engines with the engine driver. (engine.h)
	In addition, the function enables/enables the stuck checking 
------------------------------------------------------------- */
void EngineCommand(uint8_t engine_num, uint8_t speed)
{
	// Motors control
	EngineControl(engine_num, speed);
	
	// Motors status update
	if (speed == SPEED_0) // Motor stopped
	{
		StuckEnable(0);			
	}
	else // Motor active
	{
		StuckEnable(1);
		// Adjust the stuck time proportionally to the speed
		stuck_time = (( SPEED_MAX/speed ) * STUCK_TIME );
		// If the serving engine starts, its tube should be empty, 
		// so the stuck time should be higher
		if (engine_num == CONTAINER_MIX)
		{
			stuck_time = STUCK_TIME_SERVING;
		}			
	}
}

/* ------------------------------------------------------------
	When called, check if the weight has increased/decreased 
	and return 0 if the weight has increased and 1 if not.
------------------------------------------------------------ */
uint8_t StuckChecker()
{		
	int16_t actual_weight = WeightRead();
	
	//If motor 5 is on, weight should decrease
	if ((EngineOn() == CONTAINER_MIX))
	{		
		if (previous_weight > actual_weight)
			{
				previous_weight = actual_weight;
				return 0;
			}
		else
			return 1;	
	}
	// If other motor is on, weight should increase
	// If no motor is active, the value returned is 0.
	else
	{	
		if (previous_weight < actual_weight)
			{
				previous_weight = actual_weight;
				
				return 0;
			}
		else
			return 1;
	}
}	

/* ------------------------------------------------------------
	Initializes the timer 1. 
 ------------------------------------------------------------ */
void Timer1Init()
{
	// Normal mode
	//Freq: 8 M/64/2^16 = 1,91 Hz.  (0,52 s)
	TCCR1A = 0;
	TCCR1B |= 0b00000011;
}


uint8_t ErrorSell()
{
	return error_sell;
}

/* ------------------------------------------------------------	
	Overflows every 0,52 seconds and increments a counter if 
	the weight has not changed in this time; otherwise it 
	restarts the counter. 
	If the counter reaches the stuck_time, it means an engine 
	is stuck or a container empty, so it activates the stuck
	flag.
 ------------------------------------------------------------ */
ISR(TIMER1_OVF_vect)
{	
	uint8_t stuck_check = StuckChecker();
		
	// If the weight has not changed, increment the counter
	if (stuck_check) 
	{
		stuck_counter++;
	}
	//If it has changed, restart the counter.
	else
	{
		stuck_counter = 0;
	}
	// If an engine is stuck or a container empty:
	if (stuck_counter == stuck_time)
	{
		// Enable the stuck flag
		error_sell |= (1<<STUCK);
		stuck_counter = 0;
	}	 			
}

// S1: stop button
ISR(S1)
{
	// Stop all motors
	EngineCommand(CONTAINER_ALL,SPEED_0);
	// set stop flag.
	error_sell |= (1<<STOP);
}